#include "drone_unit_new.h"

using namespace std;




template <typename T>
T
OffboardControler::readParam(ros::NodeHandle &n, std::string name)
{
    T ans;
    name = node_name+"/"+name;
    if (n.getParam(name, ans))
    {
        ROS_INFO_STREAM("Loaded " << name << ": " << ans);
    }
    else
    {
        ROS_ERROR_STREAM("Failed to load " << name);
        n.shutdown();
    }
    return ans;
}

void
OffboardControler::getAllParam(ros::NodeHandle &nh) {
    node_name = ros::this_node::getName();
    id = readParam<int>(nh,"id");
    current_mode = readParam<int>(nh,"current_mode");
    current_sp = readParam<int>(nh,"current_sp");
    current_state = readParam<int>(nh,"current_state");
    baudrate = readParam<int>(nh, "baudrate");
    serial_timeout = readParam<int>(nh,"serial_timeout");
    rate = readParam<int>(nh,"rate");
    takeoff_height = readParam<float>(nh,"takeoff_height");
    init_pose_x = readParam<float>(nh,"init_pose_x");
    init_pose_y = readParam<float>(nh,"init_pose_x");
    sub_pos_topic_name = readParam<string>(nh,"sub_pos_topic_name");
    use_out_sp = readParam<int>(nh,"use_out_sp");
    out_sp_name = readParam<string>(nh,"out_sp_name");
    serial_name = readParam<string>(nh,"serial_name");
    drone_name = "drone"+to_string(id);
}

OffboardControler::OffboardControler() {
    int argc;
    char **argv = nullptr;
    ros::init(argc, argv, "drone_name");
    getAllParam(ros_nodehandle);
    while (!initSerialCom()){
        initSerialCom();
        usleep(10000);//100hz
    };

}

bool OffboardControler::initSerialCom() {
    try{
        ROS_INFO("Open Drone %d serial device",id);
        cur_ser.setPort(serial_name);
        cur_ser.setBaudrate(baudrate);
        serial::Timeout to = serial::Timeout::simpleTimeout(serial_timeout);
        cur_ser.setTimeout(to);
        cur_ser.open();
    }
    catch (serial::IOException &e)
    {
        ROS_ERROR_STREAM("Unable to open port "<<serial_name);
    }
    if (cur_ser.isOpen())
    {
        ROS_INFO_STREAM("Serial Port opened: "<<serial_name);
    } else
    {
        ROS_ERROR_STREAM("Not opened port "<<serial_name);
    }
    return cur_ser.isOpen();
}


OffboardControler oc;

void
drone_pos_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
    oc.cur_pos[0] = msg->pose.position.x;
    oc.cur_pos[1] = msg->pose.position.y;
    oc.cur_pos[2] = msg->pose.position.z;

    oc.cur_angle[0] = msg->pose.orientation.x;
    oc.cur_angle[1] = msg->pose.orientation.y;
    oc.cur_angle[2] = msg->pose.orientation.z;

    while(oc.correct_count<CORRECT_NUM){
        ++oc.correct_count;
        oc.start_angle[0] += (float)oc.cur_angle[0];
        oc.start_angle[1] += (float)oc.cur_angle[1];
        oc.start_angle[2] += (float)oc.cur_angle[2];
    }
    if(oc.correct_count==CORRECT_NUM){
        ++oc.correct_count;
        oc.start_angle[0] /=100;
        oc.start_angle[1] /=100;
        oc.start_angle[2] /=100;
    }

    oc.cur_angle[0] = oc.cur_angle[0]-oc.start_angle[0];
    oc.cur_angle[1] = oc.cur_angle[1]-oc.start_angle[1];
    oc.cur_angle[2] = oc.cur_angle[2]-oc.start_angle[2];
    ROS_INFO("POS: x: %+2.4f,y :%+2.4f,z :%+2.4f",oc.cur_pos[0],oc.cur_pos[1],oc.cur_pos[2]);
    ROS_INFO("RPY: r: %+2.4f,p :%+2.4f,y :%+2.4f",oc.cur_angle[0],oc.cur_angle[1],oc.cur_angle[2]);
    ROS_INFO("SRPY: r: %+2.4f,p :%+2.4f,y :%+2.4f",oc.start_angle[0],oc.start_angle[1],oc.start_angle[2]);
}

void
out_sp_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
    oc.sp_pos[0] = msg->pose.position.x;
    oc.sp_pos[1] = msg->pose.position.y;
    oc.sp_pos[2] = msg->pose.position.z;
    oc.sp_rpyt[0] = msg->pose.orientation.x;
    oc.sp_rpyt[1] = msg->pose.orientation.y;
    oc.sp_rpyt[2] = msg->pose.orientation.z;
    oc.sp_rpyt[3] = msg->pose.orientation.w;
}
int main(int argc,char ** argv){
    oc.de_pos_sp_pub = oc.ros_nodehandle.advertise<geometry_msgs::PoseStamped>(oc.drone_name+"/sp",1);
    oc.drone_pos_pub = oc.ros_nodehandle.advertise<geometry_msgs::PoseStamped>(oc.drone_name+"/pose",1);
    oc.drone_pos_sub = oc.ros_nodehandle.subscribe(oc.sub_pos_topic_name,1, drone_pos_cb);
    if(oc.use_out_sp==1){
        oc.out_sp_sub =  oc.ros_nodehandle.subscribe(oc.out_sp_name,1, out_sp_cb);
    }

    ROS_INFO("Open Drone %d serial device Sucessefully !!",oc.id);



        // cmd_msg.armCommand = ARM_DO_NOTHING;
        // cmd_msg.backInfo = ONLY_POSITION;
        // cmd_msg.takeoffCommand = TAKEOFF;
        // cmd_msg.offboardCommand = OFF_DO_NOTHING;
        // gcsMessageManager.sendMsg(cmd_msg);
        //
        // cmd_msg.armCommand = ARMED;
        // cmd_msg.backInfo = ONLY_POSITION;
        // cmd_msg.takeoffCommand = TAKEOFF;
        // cmd_msg.offboardCommand = TRY_IN;
        // gcsMessageManager.sendMsg(cmd_msg);
        //
        // cmd_msg.armCommand = ARMED;
        // cmd_msg.backInfo = ONLY_POSITION;
        // cmd_msg.takeoffCommand = TAKEOFF;
        // cmd_msg.offboardCommand = STAY_IN;
        // gcsMessageManager.sendMsg(cmd_msg);
        // ros::spinOnce();
        // double start = ros::Time::now().toSec();
        // float z_sp = 0.7f;

        while(ros::ok()){

            // cmd_msg.armCommand = ARMED;
            // cmd_msg.backInfo = ONLY_POSITION;
            // cmd_msg.takeoffCommand = TAKEOFF_DO_NOTHING;
            // cmd_msg.offboardCommand = TRY_IN;
            // gcsMessageManager.sendMsg(cmd_msg);

            cst_msg.sendCurrentState = SEND_NED_POSITION_RPY;
            cst_msg.position = {cur_pos[0],cur_pos[1],cur_pos[2]};
            cst_msg.rpy = {cur_angle[0],cur_angle[1],cur_angle[2]};
            gcsMessageManager.sendMsg(cst_msg);
            recv_msg = gcsMessageManager.receiveOneMsg();
            //publish
            backed_pose.header.stamp = ros::Time::now();
            backed_pose.pose.position.x = recv_msg.position[0]+init_pose_x;
            backed_pose.pose.position.y = recv_msg.position[1]+init_pose_y;
            backed_pose.pose.position.z = recv_msg.position[2];
            drone_pos_pub.publish(backed_pose);

            // if(use_out_sp==1){
            //     sp_msg.rpyt = {sp_rpyt[0],sp_rpyt[1],sp_rpyt[2],sp_rpyt[3]};
            //     sp_msg.xyz = {sp_pos[0]-init_pose_x,sp_pos[1]-init_pose_y,sp_pos[2]};
            // }else{
            // sp_msg.xyz = {0,0,z_sp};
            // sp_msg.setpointType = ATTITUDE_SP;
            // sp_msg.rpyt = {0,0,0,0.2};
            // // }
            // //
            // gcsMessageManager.sendMsg(sp_msg);
            // recv_msg = gcsMessageManager.receiveOneMsg();
            // //publish
            // backed_pose.header.stamp = ros::Time::now();
            // backed_pose.pose.position.x = recv_msg.position[0]+init_pose_x;
            // backed_pose.pose.position.y = recv_msg.position[1]+init_pose_y;
            // backed_pose.pose.position.z = recv_msg.position[2];
            // drone_pos_pub.publish(backed_pose);
            // if(ros::Time::now().toSec()-start>20.0f){
            //     z_sp = 0.0;
            // }

            loop_rate.sleep();
            ros::spinOnce();
        }
}





